#include "dev_mpu6050.h"


// MPU6050 IIC引脚初始化
void mpu6050_iic_init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    MPU6050_RCC_APBx_CMD(MPU6050_RCC_APBx, ENABLE);
	   
	GPIO_InitStructure.GPIO_Pin = MPU6050_SCL_GPIO_Pin | MPU6050_SDA_GPIO_Pin;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP ;   //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MPU6050_GPIOx, &GPIO_InitStructure);
    
    MPU6050_SCL_H();
    MPU6050_SDA_H();
}

// 简单延时
static void delay(uint16 t)
{
    while(t--);
}

// 产生IIC起始信号
void mpu6050_iic_start(void)
{
    MPU6050_SDA_OUT();
    MPU6050_SDA_H();
    MPU6050_SCL_H();
    delay(10);
    MPU6050_SDA_L();
    delay(10);
    MPU6050_SCL_L();
}

// 产生IIC停止信号
void mpu6050_iic_stop(void)
{
    MPU6050_SDA_OUT();
    MPU6050_SCL_L();
    MPU6050_SDA_L();
    delay(10);
    MPU6050_SCL_H();
    MPU6050_SDA_H();
    delay(10);
}

// 等待应答信号到来
// 返回值：1，接收应答失败
//         0，接收应答成功
uint8 mpu6050_iic_wait_ack(void)
{
    uint8 err_time = 0;
    MPU6050_SDA_IN();
    MPU6050_SDA_H(); delay(10);
    MPU6050_SCL_H(); delay(10);
    while (MPU6050_READ_SDA())
    {
        ++err_time;
        if (err_time > 250)
        {
            mpu6050_iic_stop();
            return 1;
        }
    }
    MPU6050_SCL_L();
    return 0;
}

// 产生ACK应答
void mpu6050_iic_ack(void)
{
    MPU6050_SCL_L();
    MPU6050_SDA_OUT();
    MPU6050_SDA_L();
    delay(10);
    MPU6050_SCL_H();
    delay(10);
    MPU6050_SCL_L();
}

// 不产生ACK应答
void mpu6050_iic_nack(void)
{
    MPU6050_SCL_L();
    MPU6050_SDA_OUT();
    MPU6050_SDA_H();
    delay(10);
    MPU6050_SCL_H();
    delay(10);
    MPU6050_SCL_L();
}

// IIC发送一个字节
void mpu6050_iic_send_byte(uint8 txd)
{
    uint8 t;
    MPU6050_SDA_OUT();
    MPU6050_SCL_L();
    for (t=0; t<8; ++t)
    {
        if ((txd&0x80) >> 7)    MPU6050_SDA_H();
        else                    MPU6050_SDA_L();
        txd <<= 1;
        delay(10);
        MPU6050_SCL_H();
        delay(10);
        MPU6050_SCL_L();
        delay(10);
    }
    mpu6050_iic_wait_ack();
}

// IIC读一个字节
// ack=1时，发送ACK；ack=0时，发送nACK
uint8 mpu6050_iic_read_byte(uint8 ack)
{
    uint8 i, receive = 0;
    MPU6050_SDA_IN();
    for (i=0; i<8; ++i)
    {
        MPU6050_SCL_L();
        delay(10);
        MPU6050_SCL_H();
        receive <<= 1;
        if (MPU6050_READ_SDA()) ++receive;
        delay(10);
    }
    if (!ack)
        mpu6050_iic_nack();
    else
        mpu6050_iic_ack();
    return receive;
}

//****************************************
// 定义MPU6050内部地址
//****************************************
#define	SMPLRT_DIV		0x19	//陀螺仪采样率，典型值：0x07(125Hz)
#define	CONFIG			0x1A	//低通滤波频率，典型值：0x06(5Hz)
#define	GYRO_CONFIG		0x1B	//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define	ACCEL_CONFIG	0x1C	//加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define	ACCEL_XOUT_H	0x3B
#define	ACCEL_XOUT_L	0x3C
#define	ACCEL_YOUT_H	0x3D
#define	ACCEL_YOUT_L	0x3E
#define	ACCEL_ZOUT_H	0x3F
#define	ACCEL_ZOUT_L	0x40
#define	TEMP_OUT_H		0x41
#define	TEMP_OUT_L		0x42
#define	GYRO_XOUT_H		0x43
#define	GYRO_XOUT_L		0x44	
#define	GYRO_YOUT_H		0x45
#define	GYRO_YOUT_L		0x46
#define	GYRO_ZOUT_H		0x47
#define	GYRO_ZOUT_L		0x48
#define	PWR_MGMT_1		0x6B	//电源管理，典型值：0x00(正常启用)
#define	WHO_AM_I			0x75	//IIC地址寄存器(默认数值0x68，只读)
#define	SlaveAddress	0xD0	//IIC写入时的地址字节数据，+1为读取

#define MPU_60X0_PWR_MGMT_1_REG_ADDR        0x6B  
#define MPU_60X0_USER_CTRL_REG_ADDR         0x6A  
#define MPU_60X0_SMPLRT_DIV_REG_ADDR        0x19  
#define MPU_60X0_CONFIG_REG_ADDR            0x1A  
#define MPU_60X0_GYRO_CONFIG_REG_ADDR       0x1B  
#define MPU_60X0_ACCEL_CONFIG_REG_ADDR      0x1C  
#define MPU_60X0_FIFO_EN_REG_ADDR           0x23  
  
#define MPU_60X0_RESET_REG_VALU             0x80  
#define MPU_60X0_PWR_MGMT_1_REG_VALU        0x09    // Disable temperature sensor, PLL with X axis gyroscope reference  
#define MPU_60X0_USER_CTRL_REG_VALU         0x45    // Enable FIFO. Reset FIFO and signal paths for all sensors  
#define MPU_60X0_SMPLRT_DIV_REG_VALU        0x00    // DLPF_CFG is 0x01, so Gyroscope Output Rate = 1kHz, divided by 1, still 1kHz  
#define MPU_60X0_CONFIG_REG_VALU            0x03    // 184Hz 2.0ms 188Hz 1.9ms 1kHz. So there will be 6x2 bytes new data in FIFO every 1ms  
#define MPU_60X0_GYRO_CONFIG_REG_VALU       0x08    // Gyroscope works at 500dps. If selftest is needed, REMEMBER to put this to 250dps  
#define MPU_60X0_ACCEL_CONFIG_REG_VALU      0x08    // Accelerometer works at 4g range. If selftest is needed, REMEMBER to put this to 8g range  
#define MPU_60X0_FIFO_EN_REG_VALU           0x78    // Only enable accel and gyro 

// 向I2C设备寄存器写入一个字节数据
void mpu6050_i2c_write_reg(uint8 reg_addr, uint8 reg_data)
{
    mpu6050_iic_start();                    // 发送起始信号
    mpu6050_iic_send_byte(SlaveAddress);    // 发送设备地址+写信号
    mpu6050_iic_send_byte(reg_addr);        // 内部寄存器地址
    mpu6050_iic_send_byte(reg_data);        // 内部寄存器数据
    mpu6050_iic_stop();                     // 发送停止信号
}

// 向I2C设备寄存器读取一个字节数据
uint8 mpu6050_i2c_read_reg(uint8 reg_addr)
{
    uint8 reg_data;
    mpu6050_iic_start();                    // 发送起始信号
    mpu6050_iic_send_byte(SlaveAddress);    // 发送设备地址+写信号
    mpu6050_iic_send_byte(reg_addr);        // 发送存储单元地址，从0开始
    mpu6050_iic_start();                    // 发送起始信号
    mpu6050_iic_send_byte(SlaveAddress+1);  // 发送设备地址+读信号
    reg_data = mpu6050_iic_read_byte(0);    // 读出寄存器数据	0 	NACK   1  ACK
    mpu6050_iic_stop();                     // 发送停止信号
    return reg_data;
}

// 向I2C设备寄存器读取多个字节数据
// dev_add  设备地址(低七位地址)
// reg      寄存器地址
// dat_add  数据保存的地址指针
// num      读取字节数量
void mpu6050_i2c_read_regs(uint8 reg_addr, uint8 *dat_add, uint8 num)
{
    mpu6050_iic_start();                    // 发送起始信号
    mpu6050_iic_send_byte(SlaveAddress);    // 发送设备地址+写信号
    mpu6050_iic_send_byte(reg_addr);        // 内部寄存器地址
    
    mpu6050_iic_start();                    // 发送起始信号
    mpu6050_iic_send_byte(SlaveAddress+1);  // 发送设备地址+读信号
    while(--num)
    {
        *dat_add = mpu6050_iic_read_byte(1);// 读出寄存器数据	0 	NACK   1  ACK
        dat_add++;
    }
    *dat_add = mpu6050_iic_read_byte(0);    // 读出寄存器数据	0 	NACK   1  ACK
    mpu6050_iic_stop();                     // 发送停止信号
}

// MPU6050设备初始化
void mpu6050_init(void)
{
    mpu6050_iic_init();
    
    mpu6050_i2c_write_reg(MPU_60X0_PWR_MGMT_1_REG_ADDR,     MPU_60X0_RESET_REG_VALU);
    delay(10000);
    mpu6050_i2c_write_reg(MPU_60X0_PWR_MGMT_1_REG_ADDR,     MPU_60X0_PWR_MGMT_1_REG_VALU);
    delay(10000);
    mpu6050_i2c_write_reg(MPU_60X0_USER_CTRL_REG_ADDR,      MPU_60X0_USER_CTRL_REG_VALU);  
    mpu6050_i2c_write_reg(MPU_60X0_SMPLRT_DIV_REG_ADDR,     MPU_60X0_SMPLRT_DIV_REG_VALU);  
    mpu6050_i2c_write_reg(MPU_60X0_CONFIG_REG_ADDR,         MPU_60X0_CONFIG_REG_VALU);  
    mpu6050_i2c_write_reg(MPU_60X0_GYRO_CONFIG_REG_ADDR,    MPU_60X0_GYRO_CONFIG_REG_VALU);  
    mpu6050_i2c_write_reg(MPU_60X0_ACCEL_CONFIG_REG_ADDR,   MPU_60X0_ACCEL_CONFIG_REG_VALU);  
    mpu6050_i2c_write_reg(MPU_60X0_FIFO_EN_REG_ADDR,        MPU_60X0_FIFO_EN_REG_VALU);  
    
}

#define ABS(x)      ((x) >= 0 ? (x) : -(x))
#define ACC_SENSITIVITY     (4095.875f) // 32767.0f / 8.0f
#define GYRO_SENSITIVITY    (16.388f)   // 32767.0f / 2000.0f

// 采集一次加速度计的值
void mpu6050_get_acc_data(mpu6050_data_t *mpu_data)
{
    uint8 dat[6];
    
    mpu6050_i2c_read_regs(ACCEL_XOUT_H, dat, 6);  
    mpu_data->acc.x = (int16_t)((uint16_t)dat[0]<<8 | dat[1]);
    mpu_data->acc.y = (int16_t)((uint16_t)dat[2]<<8 | dat[3]);
    mpu_data->acc.z = (int16_t)((uint16_t)dat[4]<<8 | dat[5]);
    
    mpu_data->acc_raw.x = (float)mpu6050_data.acc.x; // x轴加速度
    mpu_data->acc_raw.y = (float)mpu6050_data.acc.y; // y轴加速度
    mpu_data->acc_raw.z = (float)mpu6050_data.acc.z; // z轴加速度
}

// 采集一次陀螺仪的值
void mpu6050_get_gyro_data(mpu6050_data_t *mpu_data)
{
    uint8 dat[6];
    
    mpu6050_i2c_read_regs(GYRO_XOUT_H, dat, 6);
    mpu_data->gyro.x = (int16_t)((uint16_t)dat[0]<<8 | dat[1]);
    mpu_data->gyro.y = (int16_t)((uint16_t)dat[2]<<8 | dat[3]);
    mpu_data->gyro.z = (int16_t)((uint16_t)dat[4]<<8 | dat[5]);
    
    mpu_data->gyro.x = ABS(mpu_data->gyro.x) < 25 ? 0 : mpu_data->gyro.x;
    mpu_data->gyro.y = ABS(mpu_data->gyro.y) < 25 ? 0 : mpu_data->gyro.y;
    mpu_data->gyro.z = ABS(mpu_data->gyro.z) < 25 ? 0 : mpu_data->gyro.z;
    
    mpu_data->gyro_raw.x = mpu_data->gyro.x / GYRO_SENSITIVITY;// 翻滚角角速度
    mpu_data->gyro_raw.y = mpu_data->gyro.y / GYRO_SENSITIVITY;// 俯仰角角速度
    mpu_data->gyro_raw.z = mpu_data->gyro.z / GYRO_SENSITIVITY;// 偏航角角速度
}

// 获取原始数据
mpu6050_data_t mpu6050_data;
void mpu6050_get_raw_data(void)
{
    mpu6050_get_acc_data(&mpu6050_data);
    mpu6050_get_gyro_data(&mpu6050_data);
}
